# -*- coding: utf-8 -*-
# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
from v2x_msgs.msg import BSM, BSM_multi
from datetime import datetime, timedelta
import rospy
import pynmea2

# Connect to client
client = carla.Client('192.168.3.157', 2000)
client.set_timeout(2.0)

# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

acc_lat = {}
acc_lon = {}
acc_yaw = {}


# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps = "WGS84")     
def xy2gps(x,y,elevation):
    dis = math.sqrt(math.pow(x,2)+math.pow(y,2))
    azm = math.degrees(math.atan2(-y,x))
    #设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768,43.8334643,azm,dis)
    return lon, lat, elevation


def talker():
    pub = rospy.Publisher('bsm', BSM_multi, queue_size=10)
    rospy.init_node('bsm_msg', anonymous=True)
    rate = rospy.Rate(5) # 1hz
    while not rospy.is_shutdown():
        bsm_multi = BSM_multi()
        bsm_array = []
        count = 0
        for actor in actors:
            #遍历每一辆车：if 'vehicle' in actor.type_id:
            #遍历每一辆远车
            if 'vehicle.audi.tt' == actor.type_id:
                
                bsm_msg = BSM() 
                # idbuf
                vid = actor.id
                bsm_msg.idbuf = str(vid)

                # control
                c = actor.get_control()
                bsm_msg.TransmissionState = c.gear
                # velocity
                v = actor.get_velocity()
                speed=3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
                bsm_msg.speed = speed
                # transform
                t = actor.get_transform()
                yaw = t.rotation.yaw 
                if(yaw < -90):
                    yaw = 450 + yaw
                else:
                    yaw = 90 + yaw                
                bsm_msg.heading = yaw
                # gps
                lon, lat, elevation = xy2gps(-t.location.y,-t.location.x,t.location.z)
                bsm_msg.lat = int(lat*10000000)
                bsm_msg.lon = int(lon*10000000)
                #acc
                if(acc_lat.__contains__(vid)):
                    bsm_msg.acclat = lat - acc_lat[vid]
                else:
                    bsm_msg.acclat = 0
                
                if(acc_yaw.__contains__(vid)):
                    bsm_msg.accyaw = yaw - acc_yaw[vid]
                else:
                    bsm_msg.accyaw = 0

                if(acc_lon.__contains__(vid)):
                    bsm_msg.acclon = lon - acc_lon[vid]
                else:
                    bsm_msg.acclon = 0
                bsm_msg.accvert = 0

                acc_lat[actor.id]=lat
                acc_lon[actor.id]=lon
                acc_yaw[actor.id]=yaw

                bsm_array.append(bsm_msg)
                count = count + 1

        bsm_multi.size = count
        bsm_multi.data = bsm_array
        pub.publish(bsm_multi)
        rospy.loginfo("publish")
        rate.sleep()

if __name__ == '__main__':
    talker()
